//
// Created by zengyuhao on 25-7-20.
//
#include "robot_manager.h"
#include <thread>
#include <atomic>
#include <algorithm>
#include <mutex>
#include <condition_variable>

// Position-only gripper control. Value is position in mm (0-92), no legacy encoding.
void RobotManager::commandGripperPositionCallback(const std_msgs::msg::Float64::SharedPtr msg)
{
    const double target_position_mm = std::max(0.0, std::min(92.0, msg->data));

    if (!is_simulation_)
    {
        // Real hardware: MIT control with fixed gains and FF torque
        const double gripper_kp = 2.0;
        const double gripper_kd = 1.0;
        const double target_velocity = 0.0;

        double ff_nm = gripper_ff_torque_nm_.load();
        ff_nm = std::max(-3.0, std::min(3.0, ff_nm));

        // mm -> motor angle rad (0..92mm -> 0..5.4rad)
        double target_angle = (target_position_mm / 92.0) * 5.4;
        // Clamp to safe range
        target_angle = std::max(-0.5, std::min(5.0, target_angle));

        // Position command compensation with FF torque
        double p_cmd = target_angle - ff_nm / gripper_kp;
        p_cmd = std::max(-0.5, std::min(5.0, p_cmd));

        // Note: control_mit(kp, kd, position, velocity, torque)
        dm_->control_mit(gripper_, gripper_kp, gripper_kd, p_cmd, target_velocity, ff_nm);
    ZLOGI("[Gripper Real Pos] target=%.1f mm -> angle=%.3f rad, FF=%.2f Nm, p_cmd=%.3f", target_position_mm, target_angle, ff_nm, p_cmd);
    }
    else
    {
        // Simulation: single persistent worker thread follows latest target smoothly (velocity-limited)
        const double target_joint8_pos = std::clamp((target_position_mm / 92.0) * 0.046, 0.0, 0.046);

        // Static state for the worker
        static std::atomic<bool> worker_started{false};
        static std::atomic<double> desired_joint8_pos_m{0.0};
        static std::thread worker_thread;

        // Update target for the worker
        desired_joint8_pos_m.store(target_joint8_pos, std::memory_order_relaxed);

        if (!worker_started.load())
        {
            worker_started.store(true);
            worker_thread = std::thread([this]() {
                rclcpp::Rate rate(CONTROL_FREQ);
                const double v_max = 0.06; // m/s, velocity limit for smoothness (tunable)

                while (rclcpp::ok())
                {
                    // Read current and desired positions
                    pthread_mutex_lock(&zoros_mutex_);
                    double current_joint8 = joint_state_msg_.position[7];
                    pthread_mutex_unlock(&zoros_mutex_);

                    double target_joint8 = desired_joint8_pos_m.load(std::memory_order_relaxed);

                    // Compute limited step per control cycle
                    const double max_step = v_max / CONTROL_FREQ; // m per tick
                    double error = target_joint8 - current_joint8;
                    double step = std::clamp(error, -max_step, max_step);
                    double next_joint8 = current_joint8 + step;

                    // Apply to joint8 and mimic joint9
                    pthread_mutex_lock(&zoros_mutex_);
                    joint_state_msg_.position[7] = next_joint8;      // joint8
                    joint_state_msg_.position[8] = -next_joint8;     // joint9 (mimic)
                    pthread_mutex_unlock(&zoros_mutex_);

                    rate.sleep();
                }
            });
            worker_thread.detach();
            ZLOGI("[Gripper Sim Worker] started. New target: %.2f mm", target_position_mm);
        }
        else
        {
            ZLOGI("[Gripper Sim] Retarget to %.2f mm", target_position_mm);
        }
    }
}

void RobotManager::commandGripperFFTorqueCallback(const std_msgs::msg::Float64::SharedPtr msg)
{
    // Update FF torque for position mode only
    double ff = std::max(-3.0, std::min(3.0, msg->data));
    gripper_ff_torque_nm_.store(ff);
    ZLOGI("[Gripper FF] Updated feedforward torque: %.3f Nm", ff);
}

